from eventServer import eventServer
from rCube import RCube
from vec2 import Vec2
import math

class TCube(RCube):
    
    def __init__(self):
        RCube.__init__(self)
        self.setVar("base", "tCube")
        
        self.setVar("grav", 50.0)
        self.setVar("maxVel", 75)

        self.collisionSurfaces = {}
        self.collisionTime = 0.05
        self.setVar("mass", 100.0)
        self.setVar("friction", 0.0)
        self.setVar("frictionImmune", False)
        self.setVar("cVel", None)
        self.setVar("relMass", [0, 0, 0, 0])
        self.setVar("followTerrain", True)
        self.groundCount = 0
        self.groundCountMax = 0.1
        
    def resetPhysics(self):
        v  = self.getVar("vel")
        dv = self.getVar("dVel")
        v.x = 0
        v.y = 0
        dv.x = 0
        dv.y = 0
        self.setVar("cVel", None)
        self.collisionSurfaces.clear()
   
    def touchingGround(self):
        if(self.groundCount > 0):
            return True
        else:
            return False
        
    def updateRelativeMass(self):
        m = self.getVar("mass")
        rm = [m, m, m, m]
        for i in self.collisionSurfaces.itervalues():
            side = i["side"]
            if(side == 0):
                index = 2
            elif(side == 1):
                index = 3
            elif(side == 2):
                index = 0
            elif(side == 3):
                index = 1
            rm[index] += i["relMass"][index]
        self.setVar("relMass", rm)
        
    def correctPositionForCollision(self):
        p = self.getVar("pos")
        px = [0, 0, 0, 0]
        py = [0, 0, 0, 0]
        for i in self.collisionSurfaces.itervalues():
            if(not(i["corrected"])):
                i["corrected"] = True
                side  = i["side"]
                rMass = self.getVar("relMass")
                if(rMass[side] < self.stableMass):                    
                    dist = i["cDist"]
                    vec  = i["cVec"]
                    
                    x = (dist * vec.x)
                    if(abs(x) > abs(px[side])):
                        px[side] = x
                    y = (dist * vec.y)
                    if(abs(y) > abs(py[side])):
                        py[side] = y
                    
        for i in range(4):
            p.x -= px[i]
            p.y -= py[i]
                
    def updateVelocity(self, dt):
        v  = self.getVar("vel")
        mv = self.getVar("maxVel")
        gv = self.getVar("grav")
        fi = self.getVar("frictionImmune")
        
        fx = 0
        rx = 0
        ground = False
        
        for i in self.collisionSurfaces.itervalues():
            side = i["side"]

            if(side  == 1):
                ground = True
                x = i["rx"]
                if(abs(x) > rx):
                    rx = x
                    
                if(not(fi)):
                    x = i["fx"]
                    if(abs(x) > abs(fx)):
                        fx = x

        cv = self.getVar("cVel")
        if(not(cv == None)):
            v.x = cv.x
            v.y = cv.y
        self.setVar("cVel", None)

        if(not(fi)):
            v.x += fx

        if(self.hasVar("dVel")):
            dv = self.getVar("dVel")
            if(ground):
                v.x = ((dv.x + rx)/ (1 + math.exp(-abs(v.x + dv.x + rx) * 2)))
            else:
                v.x = (dv.x / (1 + math.exp(-abs(v.x + dv.x) * 2)))
        v.y -= (gv * dt)
 
        if(v.x > mv):
            v.x = mv
        if(v.x < -mv):
            v.x = -mv
        if(v.y > mv):
            v.y = mv
        if(v.y < -mv):
            v.y = -mv
                
    def updatePosition(self, dt):
        v = self.getVar("vel")
        p = self.getVar("pos")
        
        p.x += dt * v.x
        p.y += dt * v.y
        
    def update(self, cmd, args):
        v  = self.getVar("vel")
        p  = self.getVar("pos")
        dt = args["dtime"]
        
        RCube.update(self, cmd, args)
        
        if(self.getVar("followTerrain")):
            if(self.groundCount > -10):
                self.groundCount -= dt
            
            l = []
            for k in self.collisionSurfaces.iterkeys():
                l.append(k)
            for k in l:
                cInfo = self.collisionSurfaces[k]
                cInfo["time"] -= dt
                if(cInfo["time"] < 0):
                    self.collisionSurfaces.pop(k)
            
            self.updateRelativeMass()
            self.correctPositionForCollision()
            self.updateVelocity(dt)
            self.updatePosition(dt)
                
    def onCollision(self, dist, vec, side, sideType, above, toward, ignoreSurface, object):
        v  = self.getVar("vel")
        e1 = self.getVar("elasticity")
        
        if((sideType == "s" and toward) or (sideType == "o" and above and toward and (not ignoreSurface))):
            cInfo = None
            if(self.collisionSurfaces.has_key(object)):
                cInfo = self.collisionSurfaces[object]
                
            cv = self.getVar("cVel")
            if(cv == None):
                v1 = v
            else:
                v1 = cv

            r2 = 0
            if(object.hasVar("rot")):
                r2 = object.getVar("rot")
            v2 = Vec2()
            if(object.hasVar("vel")):
                v2 = object.getVar("vel")
                
            relMass = [self.stableMass, self.stableMass, self.stableMass, self.stableMass]
            if(object.hasVar("relMass")):
                relMass = object.getVar("relMass")
                
            if(side == 0):
                index = 2
            elif(side == 1):
                index = 3
                self.groundCount = self.groundCountMax
            elif(side == 2):
                index = 0
            elif(side == 3):
                index = 1
            m1 = self.getVar("relMass")[side]
            m2 = relMass[index]
                
            e2 = 0
            if(object.hasVar("elasticity")):
                e2 = object.getVar("elasticity")
           
            face = r2
            if(side == 0 or side == 2):
                face += math.pi/2
                
            td = face
            cd = face + (math.pi/2)
            tv = Vec2(math.cos(td), math.sin(td))
            cv = Vec2(math.cos(cd), math.sin(cd))
                
            tp1 = v1.length() * v1.norm().dot(tv)
            tp2 = v2.length() * v2.norm().dot(tv)
            tp  = tp1
            cp1 = v1.length() * v1.norm().dot(cv)
            cp2 = v2.length() * v2.norm().dot(cv)
            e = (e1 + e2)
            if(e > 0.8):
                e = 0.8

            maxV = self.getVar("maxVel")/2.0
            cpa   = ( (cp1 * (m1 - m2)) + (2 * m2 * cp2) )/ (m1 + m2)
            cpb   = abs(6 * (cpa/maxV))
            siga  = 1/(1+math.exp(-cpb))
            cp    = e * cpa * siga

            rVel  = tp2 - tp1
            f1 = self.getVar("friction")
            f2 = 0
            if(object.hasVar("friction")):
                f2 = object.getVar("friction")
            fp = 0
            if(abs(rVel) > 0):
                fp = (f1 + f2) * (rVel/abs(rVel))
            
            cx = (tp * math.cos(td)) + (cp * math.cos(cd))
            cy = (tp * math.sin(td)) + (cp * math.sin(cd))
            fx = (fp * math.cos(td))
            
            cInfo = {}
            cInfo["fx"]      = fx
            cInfo["rx"]      = v2.x
            cInfo["relMass"] = relMass
            cInfo["cDist"]   = dist
            cInfo["cVec"]    = vec
            cInfo["side"]    = side
            cInfo["corrected"] = False
            cInfo["time"]      = self.collisionTime
            
            self.setVar("cVel", Vec2(cx, cy))
            self.collisionSurfaces[object] = cInfo